// -----------------------------------------------------------------------------
// M A R I T I M E   R E S E A R C H   I N S T I T U T E   N E T H E R L A N D S
// -----------------------------------------------------------------------------
// Copyright (C) December 16, 2009 MARIN - All rights reserved - http://www.marin.nl
// -----------------------------------------------------------------------------
// Program  :
// Module   :
// File     : Geometry.cpp
// -----------------------------------------------------------------------------

#include <new>

#ifndef _WINDOWS
#include "mermaid.h"
#endif
#include <mml/Matrix.h>
#include "Geometry.h"
#include "assertplus.h"


// -----------------------------------------------------------------------------
// function Geometry::Geometry
// -----------------------------------------------------------------------------
/// constructor
/** detailed description */
// -----------------------------------------------------------------------------
Geometry::Geometry()
: m_position(0, 0, 0)
, m_attitude(mml::Matrix3d::unit())
, m_orientation(0, 0, 0)
, m_angle(0)
{
    // implementation
}


// -----------------------------------------------------------------------------
// function Geometry::~Geometry()
// -----------------------------------------------------------------------------
/// destructor
/** detailed description */
// -----------------------------------------------------------------------------
Geometry::~Geometry()
{
    // implementation
}


const mml::Vector3d& Geometry::getPosition() const
{
    return m_position;
}

const mml::Vector3d& Geometry::getOrientation() const
{
    return m_orientation;
}

const mml::Matrix3d& Geometry::getAttitude() const
{
    return m_attitude;
}


void Geometry::setPositionOrientation(const mml::Vector3d&a_position, const mml::Vector3d& a_orientation)
{
    m_position = a_position;
    m_orientation = a_orientation;
    m_attitude = mml::Matrix3d::fromTaitBryan(a_orientation(0)/*roll*/,
                                              a_orientation(1)/*pitch*/,
                                              a_orientation(2)/*yaw*/);
    m_angle = a_orientation(2) + 0.5 * PI;
    if (m_angle > 2 * PI)
    {
        m_angle -= 2 * PI;
    }
}


void Geometry::setPosition(const mml::Vector3d& a_position)
{
    mml::Vector3d orientation = getOrientation();
    setPositionOrientation(a_position, orientation);
}


void Geometry::setAngle(REAL a_angle) // y-axis is 0, x-axis is 0.5*PI
{
    m_angle = a_angle;
    mml::Vector3d position = getPosition();
    mml::Vector3d orientation = getOrientation();
    orientation(2) = a_angle - 0.5 * PI;
    if (orientation(2) < 0)
    {
        orientation(2) += 2 * PI;
    }
    setPositionOrientation(position, orientation);
}


void Geometry::setPositionAttitude(const mml::Vector3d& /*a_position*/, const mml::Matrix3d& /*a_attitude*/)
{
    ASSERTFALSE(); // not implemented in all derived classes
}


REAL Geometry::getAngle() const
{
    return m_angle;
}

/// moves pulse to another location (with same attitude)
void Geometry::move(REAL deltaX, REAL deltaY)
{
    mml::Vector3d pos = getPosition();
    pos(0) += deltaX;
    pos(1) += deltaY;
    setPosition(pos);
}


